/*************************************************************************
*
*	controller.c
*
*  Copyright 2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module performs navigational control of the platform.  
*
*
**************************************************************************
*
*  version 1.0.2, 2/2/09, SBS
*    - rename global vars to global_*
*
*  version 1.0.1, 1/30/09, SBS
*    - rename CONTROLMODE_HEADING --> CONTROLMODE_HEADINGKEEP
*    - add platformstatus_SetMode calls
*
*  version 1.0, 1/9/09, SBS
*    - initial version
*
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_engineering
*  @brief
*  This module performs navigational control of the platform.  
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>
#include <stdint.h>
#include <time.h>
#include <sys/timeb.h>
#include <pthread.h>


#include "types.h"
#include "delay.h"

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
#include "command.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "platformstatus.h"
#include "controls.h"
#include "sd84.h"
#include "trajplan.h"

#include "controller.h"

#define CLOSEENOUGH (10)


//**************************************************************************

enum modes {
	CONTROLMODE_FREEDRIFT=0, 
	CONTROLMODE_STATIONKEEP, 
	CONTROLMODE_MANUAL, 
	CONTROLMODE_HEADINGHOLD,
	CONTROLMODE_WAYPOINT,
	CONTROLMODE_CALIBRATE
};


//**************************************************************************

#define SLEEPDELAY (0.025)
#define LOOPINTERVAL (0.1)


//**************************************************************************

static UBYTE global_mode;
static WAYPT global_waypoint,global_waypoint2;
static WAYPT last_wpt, curr_wpt;
static double last_v, curr_v;
static BYTE global_rudder,global_throttle;
static WORD global_heading;

static pthread_mutex_t mode_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t waypoint_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t rudderthrottleheading_mutex = PTHREAD_MUTEX_INITIALIZER;


/**************************************************************************
*
* controller_GetStatus()
*/
/**
* @brief
* Retrieves the current rudder & throttle commanded values
*
*
**************************************************************************/
void controller_GetStatus(STATUS *s)
{
	pthread_mutex_lock( &mode_mutex );
	switch(global_mode)
	{
		case CONTROLMODE_STATIONKEEP:
		case CONTROLMODE_HEADINGHOLD:
		case CONTROLMODE_WAYPOINT:
			pthread_mutex_unlock( &mode_mutex );
			trajplan_GetStatus(s);
			
			s->last_waypoint.dValue = last_v;
			s->last_waypoint.position.dLatDeg = last_wpt.dLatDeg;
			s->last_waypoint.position.dLonDeg = last_wpt.dLonDeg;
			
			s->current_waypoint.dValue = curr_v;
			s->current_waypoint.position.dLatDeg = curr_wpt.dLatDeg;
			s->current_waypoint.position.dLonDeg = curr_wpt.dLonDeg;
			
			break;
		case CONTROLMODE_FREEDRIFT:
		case CONTROLMODE_MANUAL:
		default:
			pthread_mutex_unlock( &mode_mutex );
			pthread_mutex_lock( &rudderthrottleheading_mutex );
			s->dRudderCommand.dValue = global_rudder;
			s->dThrottleCommand.dValue = global_throttle;
			pthread_mutex_unlock( &rudderthrottleheading_mutex );

			break;
	}
	
	return;
}


/**************************************************************************
*
* controller_InitStatus()
*/
/**
* @brief
* Initializes rudder & throttle commanded values in status struct
*
*
**************************************************************************/
void controller_InitStatus(STATUS *s)
{
	s->dRudderCommand.dValue = -255;
	s->dThrottleCommand.dValue = -255;

	snprintf(s->dRudderCommand.sUnits,sizeof(s->dRudderCommand.sUnits),"%%");
	snprintf(s->dThrottleCommand.sUnits,sizeof(s->dThrottleCommand.sUnits),"%%");

	return;
}


/**************************************************************************
*
* controller_SetModeFreeDrift()
*/
/**
* @brief
* Set boat to free drift mode.  
*
*
**************************************************************************/
void controller_SetModeFreeDrift(void)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_FREEDRIFT;
	pthread_mutex_unlock( &mode_mutex );

//	platformstatus_SetMode(MODE_FREEDRIFT);

	return;
}


/**************************************************************************
*
* controller_SetModeCalibrate()
*/
/**
* @brief
* Set boat to free drift mode.  
*
*
**************************************************************************/
void controller_SetModeCalibrate(void)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_CALIBRATE;
	pthread_mutex_unlock( &mode_mutex );

	return;
}


/**************************************************************************
*
* controller_SetModeStationKeep()
*/
/**
* @brief
* Set boat to station keep mode.  
*
*
**************************************************************************/
void controller_SetModeStationKeep(WAYPT wpt)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_STATIONKEEP;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &waypoint_mutex );
	global_waypoint = wpt;
	global_waypoint2 = wpt;
	pthread_mutex_unlock( &waypoint_mutex );

//	platformstatus_SetMode(MODE_STATIONKEEP);
	
	return;
}


/********************************************************
*
* controller_SetModeManual()
*/
/**
* @brief
* Set boat to manual mode.  
*
*
**************************************************************************/
void controller_SetModeManual(BYTE rud, BYTE thr)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_MANUAL;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &rudderthrottleheading_mutex );
	global_rudder = rud;
	global_throttle = thr;
	pthread_mutex_unlock( &rudderthrottleheading_mutex );
	
//	platformstatus_SetMode(MODE_MANUAL);

	return;
}


/********************************************************
*
* controller_SetModeHeadingHold()
*/
/**
* @brief
* Set boat to heading-hold mode.  
*
*
**************************************************************************/
void controller_SetModeHeadingHold(WORD hdg, BYTE thr)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_HEADINGHOLD;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &rudderthrottleheading_mutex );
	global_heading = hdg;
	global_throttle = thr;
	pthread_mutex_unlock( &rudderthrottleheading_mutex );
	
//	platformstatus_SetMode(MODE_HEADINGKEEP);

	return;
}


/**************************************************************************
*
* controller_SetModeWaypoint()
*/
/**
* @brief
* Set boat to waypoint-achieve mode.  
*
*
**************************************************************************/
void controller_SetModeWaypoint(WAYPT wp1,WAYPT wp2)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_WAYPOINT;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &waypoint_mutex );
	global_waypoint = wp1;
	global_waypoint2 = wp2;
	pthread_mutex_unlock( &waypoint_mutex );
	
//	platformstatus_SetMode(MODE_WAYPOINT);

	return;
}


/**************************************************************************
*
* controller_mainloop()
*/
/**
* @brief
* Low-level control loop for vehicle.
*
*
**************************************************************************/
void * controller_mainloop(
		void *ptr)
{
	struct timeb starttime,endtime;
	TIMESPEC delay;
	double delta;
	STATUS s;
	COMMAND c1, c2;
	c1 = platformstatus_GetCommand();
	s = platformstatus_GetStatus();
	
	delay = delay_setdelay(SLEEPDELAY);
	

	while(1){
	
		c2 = platformstatus_GetCommand();
		if (command_CompareCommand(c1,c2))
		{
			c1 = platformstatus_GetCommand();
			s = platformstatus_GetStatus();
		}

		ftime(&starttime);
		
//		printf("mode = %d\n",global_mode);

		pthread_mutex_lock( &mode_mutex );
		
		if (global_mode == CONTROLMODE_WAYPOINT)
			controller_DealWithWaypoints(&s);
		else
		{
			last_v = -255;
			curr_v = -255;
		}
				
		switch(global_mode)
		{
			case CONTROLMODE_FREEDRIFT:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_DEBUG
							,"controller_mainloop() - Free Drift mode");
				#endif	
				
				printf("Mode is Freedrift\n");
				controls_SetRudder(0);
				controls_SetThrottle(0);

//				controls_SetRudder(0);
//				controls_SetThrottle(-100);


//				trajplan_HeadingKeep(global_heading,global_throttle);
//				trajplan_HeadingKeep(50,50);
//				controls_GetRudder();

				break;

			case CONTROLMODE_STATIONKEEP:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_DEBUG
							,"controller_mainloop() - Station Keep mode");
				#endif	
			
				trajplan_StationKeep(global_waypoint);	

				break;

			case CONTROLMODE_MANUAL:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_DEBUG
							,"controller_mainloop() - Manual mode");
				#endif		
			
				controls_SetRudder(global_rudder);
				controls_SetThrottle(global_throttle);
			
				break;

			case CONTROLMODE_HEADINGHOLD:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_DEBUG
							,"controller_mainloop() - Heading-keep mode");
				#endif		
			
				trajplan_HeadingHold(global_heading,global_throttle);
			
				break;

			case CONTROLMODE_WAYPOINT:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_DEBUG
							,"controller_mainloop() - Waypoint achieve mode");
				#endif		
			
				curr_v = 0;
				curr_wpt = s.CurrWpt;
								
				printf("Mode is waypoint\n");
				trajplan_WaypointAchieve(s.CurrWpt,s.CurrWpt);
				
				break;

			default:
				pthread_mutex_unlock( &mode_mutex );
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_CRITICAL
							,"controller_mainloop() - Unknown mode!");
				#endif	
				break;	
		}
		
		ftime(&endtime);
				
		delta = delay_deltatime(starttime,endtime);
		
		while(delta < (LOOPINTERVAL-SLEEPDELAY)){
			delay_sleep(&delay);
			ftime(&endtime);	
			delta = delay_deltatime(starttime,endtime);		
		}
		
	}

}


/**************************************************************************
*
* controller_Init()
*/
/**
* @brief
* Initialize global variables.  
*
*
**************************************************************************/
void controller_Init(WAYPT wpt,UBYTE md,BYTE rud,BYTE thr,WORD hdg)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = md;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &waypoint_mutex );
	global_waypoint = wpt;
	global_waypoint2 = wpt;
	pthread_mutex_unlock( &waypoint_mutex );

	pthread_mutex_lock( &rudderthrottleheading_mutex );
	global_rudder = rud;
	global_throttle = thr;
	global_heading = hdg;
	pthread_mutex_unlock( &rudderthrottleheading_mutex );

	switch(global_mode){
		case CONTROLMODE_FREEDRIFT:
			platformstatus_SetMode(MODE_FREEDRIFT);
			break;
			
		case CONTROLMODE_STATIONKEEP:
			platformstatus_SetMode(MODE_STATIONKEEP);
			break;
			
		case CONTROLMODE_MANUAL:
			platformstatus_SetMode(MODE_MANUAL);
			break;
			
		case CONTROLMODE_HEADINGHOLD:
			platformstatus_SetMode(MODE_HEADINGHOLD);
			break;
			
		case CONTROLMODE_WAYPOINT:
			platformstatus_SetMode(MODE_WAYPOINT);
			break;		
	}

	
	return;
}


/**************************************************************************
*
* controller_Init()
*/
/**
* @brief
* Initialize global variables.  
*
*
**************************************************************************/
void controller_InitDefault(void)
{
	pthread_mutex_lock( &mode_mutex );
	global_mode = CONTROLMODE_FREEDRIFT;
	pthread_mutex_unlock( &mode_mutex );

	pthread_mutex_lock( &waypoint_mutex );
	global_waypoint = waypoints_WaypointInit();
	global_waypoint2 = waypoints_WaypointInit();
	pthread_mutex_unlock( &waypoint_mutex );

	pthread_mutex_lock( &rudderthrottleheading_mutex );
	global_rudder = 0;
	global_throttle = 0;
	global_heading = 0;
	pthread_mutex_unlock( &rudderthrottleheading_mutex );

	platformstatus_SetMode(MODE_FREEDRIFT);
	
	return;
}


/**************************************************************************
*
* controller_Start()
*/
/**
* @brief
* Starts the controller thread.
*
*
**************************************************************************/
int controller_Start()
{
	pthread_t thread;
	int rval;

	printf("controller_StartThread()\n");
	
	rval = pthread_create( &thread, NULL, controller_mainloop, (void*) NULL);

	#ifdef ENGINEERING_USE_SD84
	sd84_Start("/dev/sd84_0");
	controls_Init();
	#endif

	return(rval);
}
/**
***************************************************************************
*
* controller_DealWithWaypoints()
*/
/**
* @brief
* When the boat is within a prescribed radius of the
*    current target waypoint, this function increments
*    the waypoint, or if there are no more waypoints, 
*    changes the boat mode to "Awaiting Instructions"
*   
*
********************************************************/
void controller_DealWithWaypoints(STATUS *s)
{
	
	double dist;
	WAYPT p;
	STATUS curr;
	static int flag;
	
	if (s->nCurrWaypt == 0)
		flag = 0;
		
	if (flag == 1)
	{
		global_mode = CONTROLMODE_FREEDRIFT;
		return;
	}
	
	curr = platformstatus_GetStatus();
	
	#ifdef DEBUG
	char temp[LOGSTRLEN];
	snprintf(temp, LOGSTRLEN, "DealWithWaypoints() - "
				"Waypoint goal is (%10.7lf,%10.7lf) @ %6.3lf m/s"
				, s->CurrWpt.dLatDeg
				, s->CurrWpt.dLonDeg
				, s->CurrWpt.dSpeedMps);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif		

	//**********
	// check if waypoint reached
	//**********
	
	p = status_WaypointFromStatus(curr) ;
	dist = waypoints_DistanceFromWaypoint(p, s->CurrWpt) ;
	printf("dist is %lf\n", dist);

	#ifdef DEBUG	
	snprintf(temp, LOGSTRLEN, 
					"DealWithWaypoints() - "
					"Distance to waypoint is %lf"
					,dist);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif
	
	if(dist < s->CurrWpt.dCloseEnoughM){
		
		#ifdef DEBUG	
		logfile_LogLevelPrint(LOGLEVEL_INFO
				,"DealWithWaypoints() - Waypoint achieved");
		snprintf(temp,LOGSTRLEN,"current waypoint is %d\n"
				,s->nCurrWaypt);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		snprintf(temp,LOGSTRLEN,"waypoint count is %d\n"
				,s->nWaypointCount);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		#endif		
		
		//printf("before: %d\n",s->nCurrWaypt);
		
		last_v = 0;
		last_wpt = s->CurrWpt;
				
		
		if((s->nCurrWaypt) < (s->nWaypointCount - 1)){
		
			(s->nCurrWaypt)++;

			// next waypoint

			#ifdef DEBUG	
			snprintf(temp,LOGSTRLEN,"advancing to waypoint #%d\n"
					,s->nCurrWaypt);
			logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
			#endif

			printf("Next Waypoint; no.: %d Count: %d\n",s->nCurrWaypt,s->nWaypointCount);
			s->CurrWpt = s->Waypoints[s->nCurrWaypt];
			
		}else{

			// finished path
			flag = 1;
			
			curr_v = -255;
			
			#ifdef DEBUG	
			snprintf(temp,LOGSTRLEN,"reached last waypoint\n");
			logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
			#endif

			printf("Finished path\n");
			global_mode = CONTROLMODE_FREEDRIFT;
		}
	}
	
	return;
}
